/*
 * @Author: Du Weixing duweixing@sgsimulation.com
 * @Date: 2024-11-08 15:59:34
 * @LastEditors: Du Weixing duweixing@sgsimulation.com
 * @LastEditTime: 2024-11-08 17:11:19
 * @FilePath: \SGFEM\DataStructure\Math\src\MathTools.cpp
 * @Description:
 *
 * Copyright (c) 2024 by 神工仿真, All Rights Reserved.
 */

#include "MathTools.h"
//#include "../include/MathTools.h"

#include <assert.h>

#include <cmath>

using namespace SG::Algebra;

// using SG::DataStructure::Common::Real;
// using SG::Algebra::Matrixd;
// using SG::Algebra::Point;
// using SG::Algebra::Vector3D;

namespace SG::Algebra
{
    void computeLocalCoordTrans (const Vector3D& xVec, const Vector3D& orien, _OUT Matrixd& T)
    {
        // 单元局部坐标系 Z 轴
        Real_t norm;
        auto   zVec = Cross (xVec, orien);
        zVec.Normalize (norm);

        // 单元局部坐标系 Y 轴
        auto yVec = Cross (zVec, xVec);

        T (0, 0) = xVec.m_x;
        T (0, 1) = xVec.m_y;
        T (0, 2) = xVec.m_z;
        T (1, 0) = yVec.m_x;
        T (1, 1) = yVec.m_y;
        T (1, 2) = yVec.m_z;
        T (2, 0) = zVec.m_x;
        T (2, 1) = zVec.m_y;
        T (2, 2) = zVec.m_z;
    }

    void computeLocalCoordTrans (const Point& point1, const Point& point2, _OUT Matrixd& T)
    {
        // 生成 X 轴基矢量
        auto xVec = Distance (point2, point1);
        assert (!xVec.IsZero ());
        Real_t tmp;
        xVec.Normalize (tmp);

        // 获取 X 轴基矢量绝对值最小分量索引
        const std::vector<Real_t> tmpVec{xVec.m_x, xVec.m_y, xVec.m_z};
        std::size_t index{0};
        if (std::fabs (tmpVec[0]) > std::fabs (tmpVec[1]))
        {
            index = 1;
        }
        if (std::fabs (tmpVec[index]) > std::fabs (tmpVec[2]))
        {
            index = 2;
        }

        // 构造辅助矢量（类似方向矢量）
        std::vector<Real_t> mVec (3);
        mVec[index] = 1.0;
        const Vector3D m{mVec[0], mVec[1], mVec[2]};

        // 依次生成 Z 轴、Y 轴基矢量
        const auto zVec = Cross (xVec, m);
        const auto yVec = Cross (zVec, xVec);

        T (0, 0) = xVec.m_x;
        T (0, 1) = xVec.m_y;
        T (0, 2) = xVec.m_z;
        T (1, 0) = yVec.m_x;
        T (1, 1) = yVec.m_y;
        T (1, 2) = yVec.m_z;
        T (2, 0) = zVec.m_x;
        T (2, 1) = zVec.m_y;
        T (2, 2) = zVec.m_z;
    }

    void projectPoint2Face (const Point& origin, const Vector3D& n, const Point& point,  _OUT Point& project)
    {
        assert (n.IsUnit());
        auto v = SG::Algebra::Distance (point, origin);
        auto tmp = v - (n * dot (v, n));
        project = Point{tmp.m_x + origin.m_x, tmp.m_y + origin.m_y, tmp.m_z + origin.m_z};
    }
}  // namespace SG::Algebra